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Abstract — To  plan  dynamic,  whole-body  motions  for  robots, 
one  conventionally  faces  the  choice  between  a  complex,  full- 
body  dynamic  model  containing  every  link  and  actuator  of  the 
robot,  or  a  highly  simplified  model  of  the  robot  as  a  point  mass. 
In  this  paper  we  explore  a  powerful  middle  ground  between 
these  extremes.  We  present  an  approach  to  generate  whole-body 
motions  using  a  simple  dynamics  model,  which  enforces  that 
the  linear  and  angular  momentum  of  the  robot  be  consistent 
with  the  external  wrenches  on  the  robot,  and  a  full-body 
kinematics  model  that  enforces  rich  geometric  constraints,  such 
as  end-effector  positioning  or  collision  avoidance.  We  obtain  a 
trajectory  for  the  robot  and  profiles  of  contact  wrenches  by 
solving  a  nonlinear  optimization  problem  (NLP).  We  further 
demonstrate  that  we  can  plan  without  pre-specifying  the  contact 
sequence  by  exploiting  the  complementarity  conditions  between 
contact  forces  and  contact  distance.  We  demonstrate  that  this 
algorithm  is  capable  of  generating  highly-dynamic  motion  plans 
with  examples  of  a  humanoid  robot  negotiating  obstacle  course 
elements  and  gait  optimization  for  a  quadrupedal  robot. 

1.  INTRODUCTION 

Humanoids  are  created  with  the  dream  of  performing  com¬ 
plex  and  dynamical  motions  like  humans.  Recent  demonstra¬ 
tions,  like  the  December  2013  Trials  of  the  DARPA  Robotics 
Challenge,  have  shown  that  while  today’s  humanoids  are 
capable  of  performing  kinematically  complex  motions  in 
uncontrolled  environments,  they  are  often  restricted  to  the 
quasi-static  regime.  One  reason  for  this  is  the  difficulty  in 
planning  complex  whole-body  dynamic  motions  at  interac¬ 
tive  rates  when  the  environment  is  not  known  a  priori. 

There  are,  broadly  speaking,  two  approaches  to  dynamic 
motion  planning  for  a  humanoid  robot.  Some  researchers 
use  trajectory  optimization  with  full-body  dynamics.  This 
approach  can  produce  beautiful  trajectories  [15]  [19]  [4], 
but  due  to  the  complexity  of  the  full-body  dynamics,  these 
optimizations  can  take  an  excessively  long  time  to  run,  and 
may  also  suffer  from  local  minima.  Thus,  this  approach  can 
become  intractable  for  complex  robots.  On  the  other  hand, 
there  exists  a  large  arsenal  of  planning  algorithms  that  use 
a  simple  dynamics  model  like  the  linear  inverted  pendulum 
[12],  [5].  With  Zero  Moment  Point  (ZMP)  as  the  stability 
criteria,  motion  plans  can  be  computed  at  interactive  rates. 
However,  there  are  some  limitations  to  this  approach.  The 
over-simplified  model  regards  the  robot  as  a  point-mass,  and 
thus  ignores  all  the  kinematics  constraints.  Moreover,  these 
models  typically  rely  on  the  assumption  that  the  center  of 
mass  (COM)  height  is  constant  (or  on  a  constant  slope),  that 
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Fig.  1:  Atlas  robot  subject  to  contact  forces  (blue  arrows) 
on  feet  and  hand,  and  contact  torque  (green  arrows)  on  left 
hand  when  it  grasps  the  hand  rail.  These  contact  wrenches 
and  the  gravitational  force  generate  the  aggregated  force 
(blue  arrow),  and  the  aggregated  torque  (green  arrow)  at  the 
Center  of  Mass  (red  sphere).  The  aggregated  force  and  the 
torque  equals  to  the  rate  of  the  centroidal  linear  and  angular 
momentum. 

the  ground  is  fiat,  and  the  robot  is  only  subject  to  unilateral 
ground  contact  forces  on  the  feet.  Thus,  the  formulation 
requires  variations  to  apply  to  walking  on  uneven  ground, 
and  it  is  not  applicable  to  more  complicated  motions  like 
jumping  and  climbing.  Additionally,  the  point-mass  model 
suggests  that  the  centroidal  angular  momentum  is  zero, 
which  is  not  valid  for  motions  requiring  fast  arm  swinging. 
As  a  result,  we  need  to  resort  to  other  stability  criteria  and 
models  to  design  complex  whole-body  motion. 

Maintaining  the  contact  wrench  sum  (CWS)  within  the 
contact  wrench  cone  (CWC)  is  proposed  as  a  universal  sta¬ 
bility  criteria  for  robot  dynamics  to  replace  the  conventional 
ZMP  [10].  It  states  that  the  aggregated  wrench  generated 
by  the  contact  and  the  gravitational  force,  should  be  equal  to 
the  rate  of  linear  and  angular  momentum  of  the  robot.  Unlike 
maintaining  the  ZMP  within  a  support  polygon,  this  criterion 
holds  for  arbitrary  motions  and  contact  profiles.  However, 
like  ZMP-based  criteria,  it  eschews  the  complex,  joint-level 
dynamics  of  a  full-body  model,  and  summarizes  the  robot’s 
dynamic  state  into  a  simple  quantity,  in  this  case  its  momenta. 
There  has  been  a  great  success  in  controlling  robots  based 
on  momenta  [14]  [13],  including  the  resolved  momentum 
control  framework  proposed  by  Kajita  et.  al.  [11]  [17].  In  the 
graphics  community,  Ye  constructs  an  abstract  model  with 
momenta  being  the  state,  and  develops  an  optimal  controller 
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to  simulate  the  character  in  the  physics  based  animation  [23]. 
The  success  of  using  momenta  in  motion  control  encourages 
us  to  apply  the  similar  idea  to  whole-body  motion  planning. 

A  key  observation  is  that  the  simple  dynamics  formu¬ 
lations,  including  ZMP  and  CWS  formulations,  can  all  be 
formulated  as  a  nonlinear  trajectory  optimization  problem. 
While  this  is  not  the  standard  formulation,  it  can  still  provide 
extremely  efficient  solutions.  Similarly,  while  the  inverse 
kinematics  problem  can  be  solved  in  closed-form  for  simple 
kinematic  chains  and  relatively  simple  constraints,  we  have 
recently  developed  a  very  fast  and  rich  inverse  kinematics 
engine  based  on  nonlinear  optimization [6].  Together,  these 
observations  highlight  a  continuum  of  algorithms  which 
range  from  using  simple  dynamics  to  full  dynamics,  and/or 
simple  kinematics  to  full  kinematics.  In  this  paper  we  explore 
a  very  powerful  middle  ground,  with  simple  dynamics  but 
rich  kinematics.  The  hope  is  that  we  can  very  rapidly  find 
feasible  trajectories  for  very  complex  tasks.  The  fear  is  that 
we  might  sacrifice  some  richness  in  the  dynamic  motions, 
however  we  demonstrate  a  variety  of  very  rich  dynamic 
behaviors  including  a  humanoid  dynamically  negotiating 
an  obstacle  course  and  dynamic  gait  optimization  for  a 
quadruped. 

This  paper  is  organized  as  follows.  In  Section  II  we  de¬ 
scribe  the  simple  dynamics  model  and  full  kinematics  model, 
as  well  as  our  formulation  of  the  nonlinear  programing 
problem.  We  also  describe  the  variation  of  our  formulation 
to  incorporate  planning  unscheduled  contact  sequence.  In 
Section  III,  we  show  our  results  on  Atlas  performing  a  variety 
of  complex  motions,  and  on  LittleDog.  We  conclude  our 
discussion  in  Section  IV. 

II.  Approach 

When  a  humanoid  robot  is  in  interacting  with  complex 
environments,  kinematic  reachability  and  collision  avoidance 
can  be  just  as  important  and  constraining  as  the  dynamic 
constraints  like  maintaining  contact  forces  inside  of  a  friction 
cone.  To  this  end,  we  use  a  full  kinematics  model  to 
enforce  geometric  contact  conditions,  and  a  dynamics  model 
that  encodes  the  relationship  between  the  contact  wrench 
(force/torque)  on  the  robot  and  the  robot’s  momenta. 

A.  Simple  dynamics  model 

Robots  with  n  joints  have  a  total  of  n  +  6  degrees  of 
freedom  (DOF),  including  joints  and  the  fioating  base.  Even 
with  full  actuation  of  the  joints,  the  six  DOFs  of  the  fioating 
base  are  un-actuated.  Those  six  degrees  of  freedom  cannot  be 
controlled  directly;  instead,  the  rates  of  those  six  DOFs  are 
determined  by  the  motion  of  the  robot’s  links  and  the  external 
wrenches  on  the  robot,  namely,  the  contact  wrenches  and 
the  gravitational  force.  Those  six  DOFs  can  be  represented 
using  the  robot’s  linear  and  angular  momentum  at  the  COM. 
A  necessary  condition  for  a  physically  tractable  motion  is 
that  the  rate  of  centroidal  linear  and  angular  momentum, 
computed  from  the  robot’s  joint  angles  and  velocities,  equals 
the  total  wrench  generated  by  the  external  contacts  and  the 


gravitational  force: 

mf  =  ^^Fj+mg  (la) 

3 

H(q,  v)  =  y](cj  -  r)  X  Fj  +  Tj  (lb) 

3 

where  m  is  the  total  mass  of  the  robot,  r  G  is  the 
COM  position,  Fj  G  is  the  contact  force  at  contact 
point,  and  g  G  is  the  gravitational  acceleration.  Eq.(la) 
is  Newton’s  second  law  enforcing  that  the  rate  of  linear 
momentum  of  the  robot  equals  the  total  external  forces. 
H(q,  v)  G  is  the  centroidal  angular  momentum  computed 
from  the  robot  posture  q  G  and  posture  velocity 

V  G  Cj  G  is  the  position  of  the  contact  point. 

Tj  G  is  the  contact  torque  at  the  contact  point.  Eq.(lb) 
enforces  that  the  rate  of  centroidal  angular  momentum  equals 
the  torque  generated  by  the  contact  wrenches  at  the  COM. 
The  centroidal  angular  momentum  can  be  computed  using 
the  method  described  in  [18] 

H(q,v)=A(q)v  (2) 

where  A(q)  G  is  the  centroidal  angular  momen¬ 

tum  matrix. 

Assuming  sufficient  control  authority  (sufficient  DOFs 
away  from  singularity  and  strong  actuators),  the  six  equa¬ 
tions  (la- lb)  are  also  sufficient  conditions  for  planning 
dynamically  feasible  motions.  Many  robots,  including  most 
humanoids,  have  actuators  for  every  internal  joint;  in  that 
case,  for  any  desired  joint  acceleration  there  is  always  a 
joint  torque  to  achieve  such  motion.  As  a  result,  if  we 
ignore  force/torque  limits  of  the  actuators,  then  we  can  ignore 
the  internal,  joint-level  dynamics  of  the  robot.  Thus  the  six 
equations  (la- lb),  which  relate  the  external  wrenches  to  the 
overall  momentum  of  the  robot,  are  necessary  and  sufficient 
to  describe  the  dynamics  of  the  robot.  This  dynamics  model 
is  much  simpler  than  the  full-body  model,  with  fewer  con¬ 
straints  (n  +  6  to  6),  and  fewer  variables,  as  the  joint  torques 
can  be  computed  subsequently  using  inverse  dynamics. 

B.  Full  kinematics  model 

In  order  to  accommodate  the  geometric  constraints  im¬ 
posed  by  interaction  between  the  robot  and  its  environment, 
we  plan  using  a  full  model  of  the  robot’s  kinematics.  This 
allows  us  to  specify  a  rich  variety  of  constraints  on  the 
robot’s  motion.  These  range  from  simple  constraints  on 
the  position/orientation  of  the  robot’s  end-effectors,  to  gaze 
constraints  between  links  of  the  robot  (“the  head  must  look 
at  the  right  hand”),  to  constraints  across  multiple  points  in 
time  (“the  right  foot  must  remain  stationary  between  times  ti 
and  ^2”),  to  collision  avoidance  constraints.  Several  of  these 
constraint  types  are  demonstrated  in  Figure  2. 

Since  we  wish  to  resolve  kinematic  constraints  at  multi¬ 
ple  instants  in  time,  where  the  joint  configuration  at  each 
instant  is  in  some  way  related  to  that  at  adjacent  instants, 
analytical  or  Jacobian  transpose  based  approaches  to  inverse 
kinematics  that  consider  the  solution  at  a  single  instant  in 


Fig.  2:  Solving  inverse  kinematics  problem  with  different 
types  of  kinematic  constraints.  The  left  foot  and  the  right 
foot  toes  are  constrained  to  lie  within  the  shaded  regions. 
A  point  (red  sphere)  on  the  right  hand  is  constrained  to  be 
within  the  shaded  bounding  box.  The  head  camera  gazes  at 
the  the  point  (red  sphere)  on  the  right  hand,  such  that  the 
point  is  within  a  cone  originated  from  the  camera,  with  15° 
being  the  half  angle  of  the  cone.  The  left  hand  orientation  is 
constrained  to  be  the  same  as  the  green  hand  drawn  by  side. 

time  [2]  are  not  sufficient.  However,  the  formulation  of 
inverse  kinematics  as  a  nonlinear  optimization  extends  quite 
naturally  to  this  situation.  The  kinematic  model  requires  n+6 
decision  variables  for  each  instant  considered  which  leads 
to  large  optimization  problems.  Fortunately,  satisfaction  of 
each  kinematic  constraints  depends  only  on  the  state  of  the 
robot  during  a  particular  interval  in  time.  Therefore  a  motion 
planning  problem  with  kinematic  constraints  will  tend  to  be 
sparse,  in  that  each  constraint  will  depend  on  only  a  small 
fraction  of  the  decision  variables. 

C.  Collision  model 

One  of  the  more  complex  kinematic  constraints  on  the 
motion  of  a  humanoid  robot  is  that  the  motion  be  collision 
free.  Our  collision  model  consists  of  n^iem  convex  collision 
geometries  each  of  which  is  attached  to  the  world  or  one  of 
the  robot’s  links  at  a  known  transform.  Let  dij  (q)  denote 
the  minimum  distance  between  the  i-th  and  j-th  collision 
elements  for  the  configuration  vector  q.  The  distance  be¬ 
tween  two  collision  elements  can  be  efficiently  computed 
for  many  classes  of  convex  geometries  with  the  Gilbert- 
Johnson-Keerthi  algorithm  (GJK)  [7].  In  this  work  we  use 
the  implementation  of  GJK  in  the  Bullet  Physics  SDK  [3]. 
let  dmin  denote  the  minimum  allowable  distance  between 
any  pair  of  collision  geometries,  and  let  dij  (q)  denote  their 
difference.  Thus  we  wish  to  enforce  that 


(3) 


where  P  C  {1, . . . ,  rieiem}  x  {1, . . . ,  n^iem)  is  the  set  of 
index  pairs  that  correspond  to  pairs  of  potentially  colliding 
geometries.  The  number  of  potential  collision  pairs  grows 
with  the  square  of  the  number  of  collision  geometries.  In 
order  to  decrease  the  number  of  collision  avoidance  con¬ 
straints  that  must  be  added  to  the  trajectory  optimization, 
we  can  combine  all  of  the  collision  pairs  using  a  hinge-loss- 
like  function.  Schulman  et.  al.  use  a  true  hinge-loss  function 
for  a  similar  purpose  [20].  Here  we  use  a  smooth  function 
7  {x)  that  is  identically  zero  for  all  positive  x,  greater  than 
zero  for  all  negative  x,  and  approaches  —x  asymptotically 
as  X  goes  to  negative  infinity: 


l{x) 


0  a;  >  0 

—xex  X  <  0 


(4) 


This  function  has  the  advantage  of  being  infinitely  differ¬ 
entiable  for  all  X.  The  overall  collision  constraint  is  given 
by 

r  (q)  =  L] 

where  c  is  a  positive  scaling  factor.  In  the  examples  shown 
here  c  was  taken  to  be  Since  each  term  of  the  sum  in 
(5)  is  non-negative,  (5)  holds  if  and  only  if  all  terms  of  that 
sum  are  zero,  which  in  turn  implies  that  (3)  holds. 


D.  Trajectory  Optimization 

To  compute  a  feasible  motion  plan,  we  transcribe  the 
differential  equations  of  the  simple  dynamics  (la- lb)  to 
algebraic  equations  and  solve  them  through  nonlinear  op¬ 
timization.  This  technique  is  widely  used  in  trajectory  op¬ 
timization  [1],  [9],  [22].  Here  we  sample  all  time- varying 
quantities  at  N  knot  points,  with  the  time  durations,  h, 
between  knot  points  being  fiexible.  The  optimization  problem 
we  formulate  contains  as  decision  variables  the  robot  state 
q,  V,  COM  position  r,  velocity  f,  acceleration  f,  contact 
positions  c,  contact  forces  F,  contact  torques  r,  centroidal 
angular  momentum  H  and  its  rate  H  at  each  knot  as  well  as 
the  time  duration  between  each  pair  of  adjacent  knot  points, 
h.  We  use  the  following  objective  for  our  optimization: 

“  qnomWlI^  +  |v[fc]|7  +  \x[k]f 

k=l 

min  \ 

ci[k],v[k],h[k]  \ 

jssa  '■w, 

H[/c],H[/c]  ^  / 

(6) 

where  |x|g  is  the  abbreviation  for  the  quadratic  cost 
x'Qx,  Q  ^  0.  The  square  bracket  [k]  means  the  sampled 
value  at  the  knot  point,  qnom  represents  a  nominal 
posture.  The  first  three  quantities  in  the  cost  are  penalization 
of  the  weighted  sum  on  the  posture  error,  joint  velocities 
and  COM  acceleration.  We  also  penalize  the  weighted  L2 
norm  of  the  contact  wrench  with  the  cost  + 

C2|rj[/c]p),where  ci,C2  are  positive  scaling  factors.  This  L2 
norm  cost  has  two  effects:  it  can  prevent  a  large  contact 


dij  —  dij  (q)  dniin  ^  ^  P 


wrenches  which  could  damage  the  robot,  and  it  also  encour¬ 
ages  the  contact  wrenches  to  be  more  evenly  distributed. 

The  constraints  for  the  optimization  problem  include  the 
dynamical  constraints  (la- lb,  2),  at  each  knot  point: 

mT[k]  =  ^  Fj  [k]  +  mg  (7  a) 

3 

H[fc]  =  y](cj  [k]  -  r[k])  X  Fj  [k]  +  Tj [k]  (7b) 

3 

H[A:]  =  A(q[A:])v[A:].  (7c) 

For  simplicity,  we  use  Euler  integration  to  approximate  the 
time  derivative  function  for  posture  q  and  centroidal  angular 
momentum  H.  For  numerical  stability,  backward-Euler  is 
adopted  in  our  formulation.  The  time  integration  constraints 
are 


Fig.  3:  Illustration  of  the  contact  point  Cj,  its  distance  (pj  to 
the  contact  surface  Sj ,  and  the  local  coordinate  frame  on  the 
tangential  surface,  with  unit  vector  tx^ty.  The  complemen¬ 
tarity  condition  holds  between  contact  distance  and  the 
normal  contact  force  F^. 


q[k]  —  q[k  —  1]  =  v[/c]/i[/c]  (7d) 

H[A:]  -  li[k  -  1]  =  il[k]h[k].  (7e) 


The  COM  position  is  approximated  using  a  piecewise 
quadratic  polynomial,  and  its  time  integration  constraints  are 


T[k]  —  T[k  —  1] 


T[k]  +  T[k  —  1] 

2 


h[k] 


(If) 


and 


f[k]  —  f[k  —  1]  =  f[k]h[k].  (7g) 

We  also  have  the  kinematic  constraints  that  compute  the 
COM  and  contact  positions  from  robot  posture 


r[k]  =  com(q[/c]) 

(7h) 

Cj[k]=Pj{q_[k]) 

(7i) 

Cj[k]  e  Sj[k] 

(7j) 

where  com  (q)  is  a  function  that  computes  the  COM  location 
given  the  robot  posture  q,  Pj(q)  is  the  forward  kinematics 
function  to  compute  the  position  of  the  contact  point  for 
configuration  q,  and  the  set  Sj  is  the  desired  contact  region 
(ground,  stepping  stones,  hand  rail,  etc). 

We  further  include  joint-limit  constraints  on  posture  q, 
constraints  on  joint  velocities,  and  constraints  on  the  contact 
wrench. 


E.  Unscheduled  Contact  Sequence 

When  designing  robot  motion  with  contact,  the  traditional 
approach  is  to  pre-specify  a  contact  mode  sequence,  for 
example,  heel  touch  ^  toe  touch  ^  heel  off  ^  toe  off,  and 
then  use  optimization  to  find  a  trajectory  for  this  fixed  mode 
sequence.  However,  the  number  of  possible  contact  modes 
grows  exponentially  with  the  number  of  contact  points,  and 
the  number  of  possible  mode  sequences  for  a  given  set  of 
contact  modes  grows  exponentially  with  the  number  of  knot 
points.  This  makes  it  hard  to  choose  a  mode  sequence  prior  to 
optimization  in  many  cases.  Optimization  methods  that  can 
search  over  all  possible  mode  sequences  at  once  are  therefore 
very  useful.  By  exploiting  the  complementarity  condition 
between  the  contact  force  and  the  distance  to  contact,  Posa 
formulates  a  direct  trajectory  optimization  problem  that  does 
not  require  a  pre-specified  contact  sequence  [19].  In  this 
paper  we  apply  the  same  idea  to  our  motion  planning 
algorithm. 

The  complementarity  constraints  on  the  contact  wrench 
and  contact  distance  are 

F^"[fc],(.j(q[fc])=0  (8a) 

kiWlVi(q[fc])  =  0  (8b) 

F”[fc]  >0,(/.j(q[fc])  >0  (8c) 


q  G  Q,  V  G  V, 


G  vjj 


(7k) 


where  Q  is  the  admissible  set  of  the  posture,  V  is  the  admis¬ 
sible  set  of  velocities.  The  set  v^j  represent  the  constraints 
on  the  contact  wrench,  for  example,  friction  cone  constraints, 
or  bounded  magnitude  on  the  contact  wrench.  We  can  also 
incorporate  additional  kinematics  constraints,  such  as  those 
described  in  subsections  II-B  and  II-C. 

This  nonlinear  optimization  problem  has  very  sparse  gra¬ 
dients,  since  most  constraints  only  depend  on  variables  at  a 
single  knot  point  or  two  adjacent  knot  points.  Such  nonlinear 
programs  with  sparse  gradients  can  be  solved  efficiently 
using  powerful  nonlinear  solvers  like  SNOPT  [8],  which  we 
use  for  the  examples  in  Section  III. 


where  F^  G  M  is  the  magnitude  of  the  normal  contact 
force  at  the  contact  point,  0j(q)  is  the  distance  of 
the  contact  point  Cj  to  the  contact  surface  Sj  (ground, 
hand  rail,  etc).  The  scalar  product  being  zero  in  equations 
(8a-8b)  means  that  either  the  body  is  not  in  contact  with 
the  environment,  and  thus  the  contact  wrench  is  zero,  or 
the  distance  between  the  body  and  the  contact  surface  is 
zero  and  therefore  the  contact  wrench  may  be  non-zero. 
The  relationship  between  the  contact  force  and  distance  is 
illustrated  in  Fig. 3. 

For  simplicity  of  analysis,  we  also  impose  the  constraint 
that  bodies  in  contact  with  the  environment  do  not  slide.  We 
can  use  the  following  complementarity  constraint  when  only 
contact  forces  exist  at  a  certain  contact  point,  and  the  contact 


torque  is  always  zero: 

^][k]{{cj[k]-cj[k-l]yt,)  =  0  (9a) 

F][k]{{c,[k]-c,[k-l]yty)=0,  (9b) 

where  tx^ty  G  are  mutually  orthogonal  unit  vectors 
on  the  tangent  surface  of  the  contact  (Fig. 3).  The  com¬ 
plementarity  constraints  (9a-9b)  state  that  if  the  normal 
contact  force  exists,  i.e,  the  body  is  in  contact,  then  the 
tangential  displacement  of  the  body  between  the  two  adjacent 
knots  must  to  be  zero;  if  the  body  moves  in  the  tangential 
directions,  then  the  normal  force  has  to  be  zero,  i.e,  the  body 
is  not  in  contact. 

When  we  want  the  solver  to  search  over  all  possible 
contact  sequences,  we  add  the  complementarity  constraints 
(8a-8b,  9a-9b)  into  the  optimization  problem  in  the  previous 
subsection.  Additionally,  the  objective  function  (6)  must  be 
changed:  it  can  no  longer  include  the  L2  norm  on  the  contact 
wrench.  This  is  due  to  the  fact  that  penalizing  the  L2  norm 
would  encourage  the  forces  to  be  distributed  more  evenly 
among  the  contact  points,  biasing  the  optimization  towards 
solutions  in  which  all  potential  contact  points  are  active  at  the 
same  knot  point.  Therefore,  the  objective  function  is  reduced 
to 

N 

“  qnomWIg,  +  +  |f[fc]|^)  h[k]. 

k=l 

(10) 

Alternatively,  we  could  also  add  the  Li  norm  of  the 
wrench  in  the  cost,  so  as  to  encourage  sparse  solutions, 
meaning  the  contact  points  tend  to  be  active  at  different 
moments. 

With  the  extra  complementarity  constraints  and  the  revised 
objective  function,  the  solver  can  search  over  all  possible 
contact  sequences. 

III.  Result 

For  our  numerical  experiments,  we  primarily  use  a  dy¬ 
namic  model  of  the  Atlas  humanoid  robot  [16],  built  by 
Boston  Dynamics,  Inc  for  use  in  the  DARPA  Robotics 
Challenge. 

A.  Jumping 

Our  first  example  is  to  command  the  robot  to  jump  off  the 
ground,  as  illustrated  in  Fig.4.  We  assign  contact  points  to 
the  four  corners  of  each  foot.  The  contact  sequence  is  fixed 
as  1)  heels  take  off  2)  toes  take  off  3)  toes  touch  ground  4) 
heels  touch  ground.  The  COM  height  that  results  from  the 
optimization  is  shown  in  Fig. 5.  Note  that  the  COM  descends 
prior  to  moving  upwards,  allowing  the  impulse  required 
for  the  jump  to  be  spread  over  a  longer  period  of  time, 
and  thereby  decreasing  the  magnitude  of  the  required  force. 
This  decrease  in  COM  height  emerges  from  optimization  by 
itself;  no  additional  constraints  were  imposed  to  produce  the 
behavior. 


Fig.  4:  Atlas  jumps  off  the  ground 


CoM  height 


Fig.  5:  The  time  trajectory  of  COM  height  for  Atlas  jumping 
and  landing.  The  markers  indicate  the  height  of  the  COM  at 
the  knot  points 

B.  Running 

Our  next  example  consists  of  generating  a  periodic  running 
gait  for  Atlas.  To  do  this,  we  plan  a  trajectory  consisting  of 
a  single  half-stride  starting  at  the  apex  of  a  flight  phase.  The 
mode  sequence  is  specified  to  be  flight,  left-stance,  left-toe- 
stance,  flight.  We  constrain  the  initial  and  final  states  relative 
to  each  other  such  that  all  quantities  are  mirrored  about  the 
robot’s  sagittal  plane.  This  yields  a  half-stride  that  can  be 
mirrored  and  concatenated  to  yield  a  full  stride.  In  addition 
to  these  periodicity  constraints  and  the  kinematic  constraints 
enforcing  the  mode  sequence,  we  specify  a  stride  length 
and  speed  (2  m  and  2  m/s  respectively),  require  a  minimum 
distance  between  collision  geometries  of  3  cm,  and  constrain 
the  robot’s  head  cameras  be  point  within  15°  of  the  robot’s 
direction  of  travel.  The  remainder  of  the  robot’s  trajectory 
is  un-constrained.  Snapshots  from  the  stance  phase  of  the 
resulting  motion  are  shown  in  Figure  6.  Figure  7  shows  the 
resulting  COM  trajectory  and  vertical  ground  reaction  force 
profile. 

Starting  from  this  periodic  gait  we  can  employ  our  col¬ 
lision  avoidance  constraints  to  generate  running  motions  in 
obstructed  environments.  Figure  8  shows  snapshots  from  a 
trajectory  which  takes  Atlas  through  the  obstructed  door 
presented  by  Schulman  et.  al.  [20]  at  2  m/s.  The  end-points 
of  this  trajectory  are  the  same  as  those  of  the  original, 
un-obstructed  half- stride,  allowing  an  immediate  return  to 


(a)  Touch-down  (b)  Mid-stance  (c)  Toe-off 

Fig.  6:  Snapshots  of  Atlas  during  stance 


- COM  height 

- NGRF 


Fig.  7:  COM  height  and  normalized  vertical  ground  reaction 
force  during  running.  The  markers  indicate  the  height 

of  the  COM  at  the  knot  points 

periodic  motion  after  traversing  the  door. 

C.  Monkey  bars 

In  this  test  case,  the  robot  jumps  up  from  a  platform  to 
grasp  a  bar,  then  makes  several  swings,  and  finally  to  lands 
on  another  platform,  as  illustrated  in  Fig. 9.  The  spacing  or 
the  height  of  the  bars  changes  between  each  swing. 

As  grasp-planning  is  outside  of  the  scope  of  this  work,  we 
do  not  use  a  full  hand  model  for  this  example.  Rather,  we 
assume  that  the  kinematic  constraint  for  grasping  consists  of 
the  center  of  the  hand  coinciding  with  the  axis  of  the  bar, 
and  a  predefined  axis  on  the  hand  being  coaxial  with  the  bar 
to  within  15°.  In  order  not  to  avoid  damaging  the  hand  with 
excessive  contact  wrenches,  we  suppose  the  magnitude  of 


Fig.  8:  Atlas  goes  through  an  obstructed  door 


////  / 


Fig.  9:  A  snapshot  of  the  monkey  bars 


the  contact  force  on  each  hand  is  bounded,  and  the  contact 
torque  is  within  a  bounding  box  centered  at  the  origin.  With 
these  assumptions,  we  compute  the  motion  in  the  monkey 
bar  tasks,  which  is  shown  in  the  attached  video. 

Unspecified  contact  sequence  for  landing:  While  it  is  sim¬ 
ple  to  determine  the  contact  sequence  for  swinging  across  the 
bars  (recall  that  the  timing  of  the  modes  is  still  determined  by 
the  optimization),  it  is  non-intuitive  to  determine  the  contact 
sequence  while  the  robot  swings  itself  forward  from  the 
last  bar  and  lands  on  the  platform.  We  compare  the  fixed 
mode  version  to  the  mode-free  version  with  complementarity 
constraints  for  the  landing  phase.  We  first  fix  the  mode 
to  be  1)  left/right  foot  toes  touch  ground  2)  left/right  foot 
heels  touch  ground.  With  the  solution  to  the  fixed  mode 
as  the  seed,  we  re-compute  the  landing  motion  using  the 
complementarity  formulation  and  free  mode  sequence.  The 
results  are  compared  in  Fig.  10  and  11.  The  contact  sequence 
and  the  robot  motion  change  significantly  after  optimization 
with  the  complementarity  constraints.  This  indicates  that  our 
algorithm  is  indeed  searching  over  different  mode  sequences. 
To  our  eyes  the  motion  obtained  from  complementarity 
formulation  looks  more  natural. 

D.  Salmon  ladder 

Our  final  humanoid  test-case  is  the  obstacle  course  element 
called  the  “salmon  ladder”  (popularized  by  the  ‘Sasuke’ 
and  now  ‘American  Ninja  Warrior’  competitions),  shown 
in  Fig.  12  and  in  the  attached  video.  Despite  the  restrictive 
kinematics  of  Atlas’  arms,  the  robot  is  able  to  launch  itself 
from  one  level  to  the  next  by  bending  its  elbows  and 
swinging  its  legs  in  much  the  same  way  as  human  athletes. 

E.  LittleDog 

Our  algorithm  is  not  restricted  to  humanoids,  it  can  be 
applied  to  a  large  class  of  robots.  To  show  the  generality 
of  our  approach,  we  compute  a  running  trot  trajectory  for 
LittleDog,  a  quadruped  robot  [21].  Snapshots  of  its  flight 
phase  are  shown  in  Fig.  13.  We  also  designed  several 
galloping,  bounding,  and  walking  gaits  for  LittleDog,  with 
very  little  variation  on  the  code.  Those  gaits  are  also  included 
in  the  attached  video. 

IV.  Discussion  and  Conclusion 

A  major  concern  in  solving  any  NLP  is  running  time. 
The  current  un-optimized  MATLAB  code  employed  in  the 


(a)  Land  with  fixed  mode  se-  (b)  Land  without  pre-specified 
quence,  left  and  right  foot  toes  mode  sequence.  The  left  heel  of 
touch  ground  first  the  left  foot  touches  ground  first 


Fig.  10:  Comparison  of  landing  posture  w/o  pre-specified 


contact  sequence 

Contact  sequence  BEFORE  optimizing  Contact  sequence  AFTER  optimizing 


Fig.  11:  The  contact  sequence  before  and  after  optimizing 
with  complementarity  constraint.  The  red  dot  indicates  the 
contact  point  is  in  contact  at  that  knot  point,  the  blue  dot 
indicates  it  is  not  in  contact.  T’  stands  for  Teft‘,  ‘r’  stands 
for  ‘right’. 


(a)  t=0 


(b)  t=0.8 


(c)  t=1.41 


Fig.  12:  Snapshots  of  climbing  salmon  ladder 


(a)  Take  off  (b)  Flight  (c)  Landing 

Fig.  13:  Snapshots  of  the  flight  phase  during  LittleDog 
running 


examples  above  usually  generates  those  motions  within 
several  minutes,  but  can  take  as  long  as  several  hours 
in  some  poorly  initialized  cases.  We  will  implement  the 
approaches  described  here  in  C++  next;  our  experience 
is  that  the  C++  code  is  about  two  orders  of  magnitude 
faster  than  the  MATLAB  code  for  NLPs  of  a  comparable 
size  to  the  ones  presented  here.  For  the  DARPA  Robotics 
Challenge  Trials,  we  developed  a  C++  implementation  of 
an  NLP-based  kinematics-only  planner  that  treated  the  same 
kinematic  constraints  as  those  employed  here.  It  generates 
motion  trajectories  interactively  with  the  user,  in  less  than 
0.2  seconds. 

We  also  wrote  a  planner  that  uses  the  full-body  dynamics 
model  for  comparison.  For  the  rich  constraints  used  in  these 
examples,  we  struggled  to  have  that  solver  return  even  a 
feasible  solution,  even  after  hours  or  days  of  computation. 
We  believe  this  failure  is  due  to  the  large  size  of  the  problem, 
and  the  presence  of  many  local  minima;  however  more  work 
is  required  to  show  that  that  is  the  case. 

In  this  paper  we  combine  a  simple  dynamics  model 
and  a  full  kinematics  model  to  generate  robots’  whole- 
body  motions.  We  believe  this  combination  encodes  the 
physical  laws  necessary  to  efficiently  generate  dynamically 
and  kinematically  feasible  motions,  giving  it  an  advantage 
over  both  the  full-body  model  and  over-simplified  models. 
To  handle  the  case  when  the  contact  mode  sequence  cannot 
be  obtained  prior  to  the  trajectory  optimization,  we  present 
a  formulation  with  complementarity  constraints,  so  that  the 
solver  can  search  over  all  possible  combinations  of  contact 
sequences  simultaneously.  In  addition  to  demonstrating  the 
effectiveness  of  our  algorithm  on  several  examples  with  a 
humanoid  robot,  we  further  show  that  our  algorithm  is  not 
restricted  to  humanoids,  but  is  applicable  to  a  much  larger 
class  of  robots. 
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